//
// Created by 24087 on 2025/10/12.
// 运动学正解文件
//

#include "kinematics.h"


/* CoreXY 运动学正解
 * 输入当前坐标和A、B电机的线路程，求解出以后的点位坐标
 */

Coordinate CoreXY_Forward_Kinematics(Coordinate cordi_now, MotorCfg_Up cfg)
{
    /*根据电机旋转方向确定方程系数*/
    float m, n, p, q = 0;
    if (cfg.direction_a == CCW)
    {
        m = 1;
        p = 1;
    }
    if (cfg.direction_a == CW)
    {
        m = -1;
        p = -1;
    }
    if (cfg.direction_b == CCW)
    {
        n = +1;
        q = -1;
    }
    if (cfg.direction_b == CW)
    {
        n = -1;
        q = +1;
    }

    /*直接计算坐标值*/
    const Coordinate cordi_target = {
        .x_value = (m * cfg.distance_a + n * cfg.distance_b) / 2.0f + cordi_now.x_value,
        .y_value = (p * cfg.distance_a + q * cfg.distance_b) / 2.0f + cordi_now.y_value
    };

    return cordi_target;
}

/*  CoreXY结构运动学逆解器
 *  输入当前坐标和目标坐标，返回两个电机的配置文件，包括运动方向和线路程(mm)
 */
MotorCfg_Up CoreXY_Inverse_Kinematics(Coordinate cordi_now, Coordinate cordi_target)
{
    float m, n, p, q, alpha_deg = 0;
    MotorCfg_Up cfg;
    const float alpha_rad = atan2f(cordi_target.y_value - cordi_now.y_value, cordi_target.x_value - cordi_now.x_value);
    alpha_deg = alpha_rad * 180.0f / M_PI;
    if (alpha_deg < 0)
    {
        alpha_deg += 360.0f;
    }

    if ((alpha_rad >= 0.0f && alpha_deg < 45.0f) || (alpha_deg >= 315.0f && alpha_deg <= 360.0f))
    {
        cfg.direction_a = CCW;
        cfg.direction_b = CCW;
    }

    if (alpha_deg >= 45.0f && alpha_deg <= 135.0f)
    {
        cfg.direction_a = CCW;
        cfg.direction_b = CW;
    }

    if (alpha_deg >= 135.0f && alpha_deg <= 225.0f)
    {
        cfg.direction_a = CW;
        cfg.direction_b = CW;
    }

    if (alpha_deg >= 225.0f && alpha_deg <= 315.0f)
    {
        cfg.direction_a = CW;
        cfg.direction_b = CCW;
    }

    if (cfg.direction_a == CCW)
    {
        m = 1;
        p = 1;
    }
    if (cfg.direction_a == CW)
    {
        m = -1;
        p = -1;
    }
    if (cfg.direction_b == CCW)
    {
        n = +1;
        q = -1;
    }
    if (cfg.direction_b == CW)
    {
        n = -1;
        q = +1;
    }

    cfg.distance_a = 2 * ((cordi_target.x_value - cordi_now.x_value) + (cordi_target.y_value - cordi_now.y_value)) / (m + p);
    cfg.distance_b = 2 * ((cordi_target.x_value - cordi_now.x_value) - (cordi_target.y_value - cordi_now.y_value)) / (n - q);

    return cfg;
}
